#ifndef _MY_SIMULATION_H_

#define _MY_SIMULATION_H_

// Local Includes

// Bullet Includes
#include <LinearMath/btVector3.h>

// System Includes
#include "vector"
#include "set"

typedef enum {
    push_up    = 0, // Up push motion
    push_right = 1, // Right push motion
    push_down  = 2, // Down push motion
    push_left  = 3, // Left push motion
    push_num   = 4  // Number of pushes
} MySimPushDir;


class config
{

   public:
	  unsigned int      shape;
	  float             aInitShapePos[3];
      float             aInitMarkerPos[3];
	  float				initShapeAng;
	  float             initMarkerAng;

    config( unsigned int s, float sP[], float sA, float mP[], float mA )
      {
		shape = s;
		for(int i=0;i<3;i++){
			aInitShapePos[i] = sP[i];
			aInitMarkerPos[i] = mP[i];
		}
	    initShapeAng = sA; 
       	initMarkerAng = mA;
	  } 

    config() 
	{
        shape =0; 
		for(int i=0;i<3;i++){
			aInitShapePos[i] = 0.0f;
			aInitMarkerPos[i] = 0.0f;
		}
        initShapeAng=0.0f; 
       	initMarkerAng=0.0f;
	}  

    config( const config& rhs ) 
	{
		shape=rhs.shape;
		for(int i=0;i<3;i++){
			aInitShapePos[i] = rhs.aInitShapePos[i];
			aInitMarkerPos[i] = rhs.aInitMarkerPos[i];
		}
        initShapeAng=rhs.initShapeAng; 
        initMarkerAng= rhs.initMarkerAng;  
	}

	

};

class obj_act
{
  public:
    unsigned int         iOrient; // Orientation
    unsigned int         iPos;    // Position of push
    unsigned int         iDir;    // Direction of push
    unsigned int         iDis;    // Distance of push
    mutable unsigned int iNumAct; // Number of times action executed
    mutable unsigned int iSucAct; // Number of time successful

    obj_act( unsigned int iO, unsigned int iP, unsigned int iR, unsigned int iS )
      : iOrient(iO), 
        iPos(iP), 
        iDir(iR), 
        iDis(iS), 
        iNumAct(0), 
        iSucAct(0) {}

    obj_act() 
      : iOrient(0), 
        iPos(0), 
        iDir(0), 
        iDis(0), 
        iNumAct(0), 
        iSucAct(0) {}

    obj_act( const obj_act& rhs ) 
      : iOrient(rhs.iOrient), 
        iPos(rhs.iPos), 
        iDir(rhs.iDir), 
        iDis(rhs.iDis), 
        iNumAct(rhs.iNumAct), 
        iSucAct(rhs.iSucAct) {}

	void clear() 
	  { memset(this,0,sizeof(obj_act)); }

    
    obj_act& operator=( const obj_act& rhs )
      { 
        iOrient = rhs.iOrient; 
        iPos    = rhs.iPos; 
        iDir    = rhs.iDir; 
        iDis    = rhs.iDis;
        iNumAct = rhs.iNumAct; 
        iSucAct = rhs.iSucAct;
        return *this;
      }

    bool operator==( const obj_act& rhs ) const
      { 
        return iOrient == rhs.iOrient && 
               iPos    == rhs.iPos    && 
               iDir    == rhs.iDir    && 
               iDis    == rhs.iDis;
      }

    bool operator()(const obj_act& lhs, const obj_act& rhs) const
      {
        // 
        if ( lhs.iOrient != rhs.iOrient )
            return  lhs.iOrient < rhs.iOrient;

        //
        if ( lhs.iPos != rhs.iPos )
            return lhs.iPos < rhs.iPos;

        //
        if ( lhs.iDir != rhs.iDir )
            return lhs.iDir < rhs.iDir;

        //
        return lhs.iDis  < rhs.iDis;
      }
};

class obj_res
{
  public:
    float                               fMotDis;
    float                               fMotAng;
    float                               fOriAng;  // Change in orientation
    std::set<obj_act,obj_act>::iterator iterAct;  // Associated action

    obj_res( float fMD, float fMA, float fOA )
      : fMotDis(fMD), fMotAng(fMA), fOriAng(fOA), iterAct() {}
    obj_res() 
      : fMotDis(0.0f), fMotAng(0.0f), fOriAng(0.0f), iterAct() {}
    obj_res( const obj_res& rhs ) 
      : fMotDis(rhs.fMotDis), fMotAng(rhs.fMotAng), fOriAng(rhs.fOriAng), iterAct(rhs.iterAct) {}

	void clear() 
	  { memset(this,0,sizeof(obj_res)); }
    
    obj_res& operator=( const obj_res& rhs )
      { 
        fMotDis = rhs.fMotDis; 
        fMotAng = rhs.fMotAng; 
        fOriAng = rhs.fOriAng;
        iterAct = rhs.iterAct;
        return *this;
      }

    bool operator==( const obj_res& rhs ) const
      { 
          return fMotDis == rhs.fMotDis && 
                 fMotAng == rhs.fMotAng &&   
                 fOriAng == rhs.fOriAng;
      }

    bool isValid() const
      { 
        return (fMotDis >  0.1f && fMotDis < 6.0f) || fOriAng > 0.1f; 
      }

    static bool lessOri( const obj_res& lhs, const obj_res& rhs )
      {
          if ( lhs.iterAct->iOrient != rhs.iterAct->iOrient )
              return lhs.iterAct->iOrient < rhs.iterAct->iOrient;

          if ( lhs.fOriAng != rhs.fOriAng )
              return lhs.fOriAng < rhs.fOriAng;

          return lhs.fMotDis < rhs.fMotDis;
      }

    static bool lessMot( const obj_res& lhs, const obj_res& rhs )
      {
          if ( lhs.iterAct->iOrient != rhs.iterAct->iOrient )
              return lhs.iterAct->iOrient < rhs.iterAct->iOrient;

          if ( lhs.fMotAng != rhs.fMotAng )
              return lhs.fMotAng < rhs.fMotAng;

          return lhs.fMotDis > rhs.fMotDis;
      }
};

class MySimulation
{
  public:
      MySimulation() 
        : _iCurObj(0), _iMaxDis(180), _iMaxAng(360), _bOpt(false) {}
      ~MySimulation() {}

      void reset( unsigned int iObj );
	  bool addResults( const obj_act &rAct, const obj_res &rRes );
      
      const obj_res& obtainClosetResult( unsigned int     iOrient,             // Current orientation 
                                         float            fMarkerOri,          // Orientation to the marker
                                         float            fMarkerDis,          // Distance to the marker
                                         float            fMarkerAng ) const;  // Angle to the marker

      void serialize( const char* fname );
      void deserialize( const char* fname );
	  bool readFile();
	  void setPosition(float a,float b, float c, config &con, int x);

      void test ();
      void test2();

	   std::vector<config>       _vConfig;

  protected:

      void _optimizeResults();

      unsigned int              _iCurObj;
      unsigned int              _iMaxDis;
      unsigned int              _iMaxAng;

	               
      std::set<obj_act,obj_act> _vAct;
      std::vector<obj_res>      _vRes;
	 
	  
      bool                      _bOpt;
      std::vector<obj_res>      _vResMot;
      std::vector<obj_res>      _vResOri;
      unsigned int              _aObjOri[361];
};

#endif // _MY_SIMULATION_H_